#include "track_3d.h"

/**
 * 初始化三维航迹库
 */
void Track3D::track3d_init(Tracks3D tracks[MAX_TRACKS_3D])
{
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        tracks[i].id = i;
        tracks[i].life = 0;
        tracks[i].max_life = MAX_LIFE_3D;
        tracks[i].is_valid = false;
        // 初始化卡尔曼滤波器（采样时间1s，过程噪声0.1，测量噪声1.0）
        kalman3d_init(&tracks[i].kf, 1.0, 0.1, 1.0);
    }
}

/**
 * 计算三维空间中两点的欧氏距离
 */
double Track3D::distance3d(double x1, double y1, double z1, double x2, double y2, double z2)
{
    return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2) + pow(z1 - z2, 2));
}

/**
 * 三维数据关联（最近邻法+波门限制）
 * 输入：点迹数组、点迹数量、航迹库
 */
void Track3D::data_association3d(RadarPoint3D *points, const int point_num, Tracks3D tracks[MAX_TRACKS_3D])
{
    bool assigned[MAX_TRACKS_3D] = {false}; // 标记航迹是否已匹配
    int associations[point_num];            // 点迹匹配的航迹索引（-1表示未匹配）
    for (int i = 0; i < point_num; i++)
        associations[i] = -1;

    // 1. 遍历点迹，寻找匹配的航迹（最近邻+波门限制）
    for (int i = 0; i < point_num; i++)
    {
        double x, y, z;
        polar3d_to_cartesian(&points[i], &x, &y, &z); // 点迹转直角坐标

        double min_dist = 1e9;
        int best_track = -1;

        // 遍历有效航迹，计算与预测位置的距离
        for (int j = 0; j < MAX_TRACKS_3D; j++)
        {
            if (!tracks[j].is_valid || assigned[j])
                continue;

            // 航迹预测位置（卡尔曼预测后的x, y, z）
            double pred_x = tracks[j].kf.x[0];
            double pred_y = tracks[j].kf.x[1];
            double pred_z = tracks[j].kf.x[2];

            // 计算三维欧氏距离，小于波门阈值视为潜在匹配
            double dist = distance3d(x, y, z, pred_x, pred_y, pred_z);
            if (dist < min_dist && dist < GATE_THRESHOLD)
            {
                min_dist = dist;
                best_track = j;
            }
        }

        if (best_track != -1)
        {
            associations[i] = best_track;
            assigned[best_track] = true;
        }
    }

    // 2. 更新匹配的航迹，未匹配的航迹寿命递减
    for (int j = 0; j < MAX_TRACKS_3D; j++)
    {
        if (!tracks[j].is_valid)
            continue;

        bool matched = false;
        for (int i = 0; i < point_num; i++)
        {
            if (associations[i] == j)
            {
                // 匹配成功：更新航迹
                double x, y, z;
                polar3d_to_cartesian(&points[i], &x, &y, &z);
                kalman3d_update(&tracks[j].kf, x, y, z); // 卡尔曼校正
                tracks[j].life = (tracks[j].life < MAX_LIFE_3D) ? tracks[j].life + 1 : MAX_LIFE_3D;
                matched = true;
                break;
            }
        }

        if (!matched)
        {
            // 未匹配：寿命递减
            tracks[j].life--;
            if (tracks[j].life <= 0)
            {
                tracks[j].is_valid = false; // 寿命耗尽，标记为无效
            }
        }
    }

    // 3. 为未匹配的点迹创建新航迹
    for (int i = 0; i < point_num; i++)
    {
        if (associations[i] == -1)
        {
            // 寻找空闲航迹ID
            int free_idx = -1;
            for (int j = 0; j < MAX_TRACKS_3D; j++)
            {
                if (!tracks[j].is_valid)
                {
                    free_idx = j;
                    break;
                }
            }
            if (free_idx == -1)
                continue; // 航迹库已满

            // 初始化新航迹
            double x, y, z;
            polar3d_to_cartesian(&points[i], &x, &y, &z);
            tracks[free_idx].kf.x[0] = x;     // 初始x
            tracks[free_idx].kf.x[1] = y;     // 初始y
            tracks[free_idx].kf.x[2] = z;     // 初始z
            tracks[free_idx].kf.x[3] = 0;     // 初始vx
            tracks[free_idx].kf.x[4] = 0;     // 初始vy
            tracks[free_idx].kf.x[5] = 0;     // 初始vz
            tracks[free_idx].life = 1;        // 初始寿命
            tracks[free_idx].is_valid = true; // 标记为有效
        }
    }
}


/**
 * 处理一帧三维雷达数据（主流程）
 */
void Track3D::radar3d_track_process(RadarPoint3D *points, int point_num, Tracks3D tracks[MAX_TRACKS_3D])
{
    // 1. 对所有有效航迹进行预测
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        if (tracks[i].is_valid)
        {
            kalman3d_predict(&tracks[i].kf);
        }
    }

    // 2. 数据关联与航迹更新
    data_association3d(points, point_num, tracks);

    // 3. 输出有效航迹状态
    printf("=== track result ===\n");
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        if (tracks[i].is_valid)
        {
            printf("track ID: %d\n", tracks[i].id);
            printf("  position: (%.2f, %.2f, %.2f) m\n",
                   tracks[i].kf.x[0], tracks[i].kf.x[1], tracks[i].kf.x[2]);
            printf("  speed: (%.2f, %.2f, %.2f) m/s\n",
                   tracks[i].kf.x[3], tracks[i].kf.x[4], tracks[i].kf.x[5]);
            printf("  life: %d\n", tracks[i].life);
            printf("\n\n");
        }
    }
}
